﻿#include "Measure_TS.h"
#include "qdebug.h"
Measure_TS::Measure_TS()
{
	CameraParameters.Clear();
	CameraParameters[0] = 0;
	CameraParameters[1] = -0.940741;
	CameraParameters[2] = 1.14751e-005;
	CameraParameters[3] = 1.14709e-005;
	CameraParameters[4] = 891.283;
	CameraParameters[5] = 943.919;
	CameraParameters[6] = 2000;
	CameraParameters[7] = 1500;
//	GenRadialDistortionMap(&Map, CameraParameters, CameraParameters, "bilinear");
}
int Measure_TS::M_TS(HObject Img, HTuple ModelId, HTuple &OutPara, HObject &OutImg)
{

    int P_Type = m_part.para21;
    int flag=0;

    //1号相机
    float para1=m_part.para1;
    float para2=0;


    //——————————————————————垂直方向
    //补偿值
    float para3 = m_part.para3;
    //标准值
    float para4 = m_part.para4;
    //上限值
    float para5 = m_part.para5;
    //下限值
    float para6 = m_part.para6;

    //——————————————————————水平方向
    //补偿值
    float para7 = m_part.para7;
    //标准值
    float para8 = m_part.para8;
    //上限值
    float para9 = m_part.para9;
    //下限值
    float para10 = m_part.para10;


    //------------------图像处理

    //粗定定位阈值
    float para17=m_part.para17;
    //找边sigma
    float para19=m_part.para19;
    //找边阈值
    float para20=m_part.para20;



    // Local iconic variables
    HObject  Image, Region;
    HObject  ROI_0, ImageReduced, ImageRotate;
    HObject  RegionTrans, Rectangle, ImageR, Edges;
    HObject  Cross, RegionDilation, ImgReduce, Rectangle1;
    HObject  ImageReduced1, Edges1, Rectangle2, ImageReduced2;
    HObject  Edges2;

    // Local control variables
    HTuple  CameraParameters, CameraPose;
    HTuple  DD1, DD2;
    HTuple  Length2, M_R, M_C, M_A, M_S;
    HTuple  Row, Column, Value, Length1;
    HTuple  Area, C_R, C_C, H, W, Max_R, Max_C;
    HTuple  Col, Max, Indices, RowBegin1, ColBegin1;
    HTuple  RowEnd1, ColEnd1, Nr1, Nc1, Dist1;
    HTuple  Index1, MetrologyHandle1, Index, Parameter;
    HTuple  Line1, Line2, RowBegin2, ColBegin2;
    HTuple  RowEnd2, ColEnd2, Nr2, Nc2, Dist2;
	HTuple  MetrologyHandle2, Line3, D1, D2, D3, Min;




   /* CameraParameters.Clear();
    CameraParameters[0] = 0;
    CameraParameters[1] = -0.940741;
    CameraParameters[2] = 1.14751e-005;
    CameraParameters[3] = 1.14709e-005;
    CameraParameters[4] = 891.283;
    CameraParameters[5] = 943.919;
    CameraParameters[6] = 2000;
    CameraParameters[7] = 1500;*/
  /*  CameraPose.Clear();
    CameraPose[0] = -0.00709736;
    CameraPose[1] = -0.00755097;
    CameraPose[2] = 1.00222;
    CameraPose[3] = 357.262;
    CameraPose[4] = 359.28;
    CameraPose[5] = 0.546978;
    CameraPose[6] = 0;*/
    //GenRadialDistortionMap(&Map, CameraParameters, CameraParameters, "bilinear");
    DD1 = HTuple();
    DD2 = HTuple();
    try
    {
		Flag11 = true;
		Flag22 = true;
		Flag1 = true;
		Flag2 = true;
        OutImg=Img;
        MapImage(Img, Map, &Image);
        Threshold(Image, &Region, 0, para17);
        GenRectangle1(&ROI_0, 285.145, 442.859, 897.449, 1603.02);
        ReduceDomain(Image, ROI_0, &ImageReduced);
        Threshold(ImageReduced, &Region, 0, para17);
        Connection(Region, &Region);
        SelectShapeStd(Region, &Region, "max_area", 70);
        SmallestRectangle2(Region, &Row, &Column, &Value, &Length1, &Length2);

        if (0 != (Value>(PI/2)))
        {
            RotateImage(Image, &ImageRotate, (((PI/2)-Value)*180)/PI, "constant");
        }
        else
        {
            RotateImage(Image, &ImageRotate, ((-Value)*180)/PI, "constant");
        }

        FindShapeModel(ImageRotate, ModelId, HTuple(0).TupleRad(), HTuple(180).TupleRad(), 0.5, 1, 0.5, "least_squares", 0, 0.9, &M_R, &M_C, &M_A, &M_S);

        if (0 != (M_S>0.8))
        {
            Threshold(ImageRotate, &Region, 0, para17);
            Connection(Region, &Region);
            SelectShapeStd(Region, &Region, "max_area", 70);
            ShapeTrans(Region, &RegionTrans, "rectangle2");
            AreaCenter(RegionTrans, &Area, &C_R, &C_C);
            RegionFeatures(RegionTrans, "height", &H);
            RegionFeatures(RegionTrans, "width", &W);
            RegionFeatures(RegionTrans, "row2", &Max_R);
            RegionFeatures(RegionTrans, "column2", &Max_C);



            if (0 != (HTuple(P_Type==135).TupleOr(P_Type==138)))
            {
                GenRectangle2(&Rectangle, C_R-(H/2), C_C+(W/2), 0, 50, 60);
            }
            else if (0 != (P_Type==139))
            {
                GenRectangle2(&Rectangle, C_R-(H/2), C_C-(W/2), 0, 50, 60);
            }
            else if (0 != (P_Type==142))
            {
                GenRectangle2(&Rectangle, C_R+(H/2)-40, C_C+(W/2), 0, 50, 60);
            }



            ReduceDomain(ImageRotate, Rectangle, &ImageR);
            EdgesSubPix(ImageR, &Edges, "canny", 3, 20, 40);
            SelectContoursXld(Edges, &Edges, "contour_length", 50, 200, -0.5, 0.5);
            GetContourXld(Edges, &Row, &Col);

			if (0 != (HTuple(HTuple(P_Type == 135).TupleOr(P_Type == 142)).TupleOr(P_Type == 138)))
			{
				TupleMax(Col, &Max);
				TupleFind(Col, Max, &Indices);
			}
			else if (0 != (P_Type == 139))
			{
				TupleMin(Col, &Min);
				TupleFind(Col, Min, &Indices);
			}


            DilationRectangle1(RegionTrans, &RegionDilation, 20, 20);
            ReduceDomain(ImageRotate, RegionDilation, &ImgReduce);


            GenRectangle2(&Rectangle1, C_R, C_C, 0, W/4, (H/2)+30);

            ReduceDomain(ImgReduce, Rectangle1, &ImageReduced1);
            EdgesSubPix(ImageReduced1, &Edges1, "canny", 3, 20, 40);
            UnionAdjacentContoursXld(Edges1, &Edges1, 10, 1, "attr_keep");
            SelectContoursXld(Edges1, &Edges1, "contour_length", (W/2)-100, (W/2)+50,
                              -0.5, 0.5);
            FitLineContourXld(Edges1, "tukey", -1, 0, 5, 2, &RowBegin1, &ColBegin1,
                              &RowEnd1, &ColEnd1, &Nr1, &Nc1, &Dist1);
            for (Index1=0; Index1<=1; Index1+=1)
            {

                CreateMetrologyModel(&MetrologyHandle1);
                AddMetrologyObjectGeneric(MetrologyHandle1, "line", ((HTuple(RowBegin1[Index1]).TupleConcat(HTuple(ColBegin1[Index1]))).TupleConcat(HTuple(RowEnd1[Index1]))).TupleConcat(HTuple(ColEnd1[Index1])),
                                          20, 5, 3, para17, HTuple(), HTuple(), &Index);
                ApplyMetrologyModel(ImageRotate, MetrologyHandle1);
                GetMetrologyObjectResult(MetrologyHandle1, "all", "all", "result_type",
                                         "all_param", &Parameter);
                ClearMetrologyModel(MetrologyHandle1);
                if (0 != (Index1==0))
                {
                    Line1 = Parameter;
                }
                else
                {
                    Line2 = Parameter;
                }
            }


            if (0 != (HTuple(HTuple(P_Type==135).TupleOr(P_Type==142)).TupleOr(P_Type==138)))
            {
                GenRectangle2(&Rectangle2, C_R, C_C-(W/2), 0, 100, H/4);
            }
            else if (0 != (P_Type==139))
            {
                GenRectangle2(&Rectangle2, C_R, C_C+(W/2), 0, 100, H/4);
            }

            ReduceDomain(ImgReduce, Rectangle2, &ImageReduced2);
            EdgesSubPix(ImageReduced2, &Edges2, "canny", 3, 20, 40);

            UnionAdjacentContoursXld(Edges2, &Edges2, 10, 1, "attr_keep");

            SelectContoursXld(Edges2, &Edges2, "contour_length", (H/2)-50, (H/2)+50,
                              -0.5, 0.5);

            FitLineContourXld(Edges2, "tukey", -1, 0, 5, 2, &RowBegin2, &ColBegin2,
                              &RowEnd2, &ColEnd2, &Nr2, &Nc2, &Dist2);
            CreateMetrologyModel(&MetrologyHandle2);
            AddMetrologyObjectGeneric(MetrologyHandle2, "line", ((RowBegin2.TupleConcat(ColBegin2)).TupleConcat(RowEnd2)).TupleConcat(ColEnd2),
                                      20, 5, 3, para17, HTuple(), HTuple(), &Index);
            ApplyMetrologyModel(ImageRotate, MetrologyHandle2);
            GetMetrologyObjectResult(MetrologyHandle2, "all", "all", "result_type",
                                     "all_param", &Parameter);
            ClearMetrologyModel(MetrologyHandle2);
            Line3 = Parameter;

            DistancePl(HTuple(Line1[0]), HTuple(Line1[1]), HTuple(Line2[0]), HTuple(Line2[1]),
                    HTuple(Line2[2]), HTuple(Line2[3]), &D1);
            DistancePl(HTuple(Line1[2]), HTuple(Line1[3]), HTuple(Line2[0]), HTuple(Line2[1]),
                    HTuple(Line2[2]), HTuple(Line2[3]), &D2);
            DistancePl(HTuple(Row[Indices]), HTuple(Col[Indices]), HTuple(Line3[0]),
                    HTuple(Line3[1]), HTuple(Line3[2]), HTuple(Line3[3]), &D3);
            TupleMean(D3,&D3);
            DD1 = ((D1+D2)/2)*para1+para2+para3;
            DD2 = D3*para1+para2+para7;
            DD1=0;
            if(!((DD1<para4+para5)&&(DD1>para4-para6)))
            {
				if (DD1 < (para4 - para6))
				{
					Flag11 = false;
				}
				Flag1 = false;
                flag = 0;
            }
            if(!((DD2<para8+para9)&&(DD2>para8-para10)))
            {
				if (DD2 < (para8 - para10))
				{
					Flag22 = false;
				}
                Flag2=false;
                flag = 1;
            }
            TupleConcat(OutPara, DD1, &OutPara);
            TupleConcat(OutPara, DD2, &OutPara);
            return flag;
        }
        else
        {
            Flag1=false;
            Flag2=false;
            OutPara.Append(0).Append(0);
            return 2;
        }
    }
    catch(HException ex)
    {
		Flag1 = false;
        Flag2=false;
        OutPara.Append(0).Append(0);
        return 2;
    }







/*

    HObject  ModelContours, Region;
    HObject  Edges1, ImageRotate, ContoursAffineTrans;
    HObject  Region1, Rectangle, RegionIntersection;
    HObject  ROI_1, ROI_2, ROI_3;
    HObject  ImageReduced1, ERegion1, EndPoints1;
    HObject  JuncPoints1, ConnectedRegions1;
    HObject  ImageReduced2, Edges2, ERegion2, EndPoints2;
    HObject  JuncPoints2, ConnectedRegions2, ImageReduced3;
    HObject  Edges3, ERegion3, EndPoints3, JuncPoints3;
    HObject  ConnectedRegions3 ;

    // Local control variables
	HTuple   Row, Column, Value, Index, Length;
    HTuple  Length1, Length2, Angle, Score, HomMat2D;
    HTuple  Value1, Value2,Value3, R, C, Row2;

    HTuple  Column2, r1, c1, line1, r2, c2;
    HTuple  line2, r3, c3, line3;
    HTuple  MetrologyHandle1, Index1,Line1;
    HTuple  MetrologyHandle2, Line2, MetrologyHandle3;
    HTuple  Line3, Distance1, D2, D3;
    HTuple  Distance2;


    try
    {


        GetShapeModelContours(&ModelContours, ModelId, 1);
        OutImg=Img;
        OutPara.Clear();
        Threshold(Img, &Region, 0, para17);
        Connection(Region,&Region);
        SelectShapeStd(Region, &Region, "max_area", 70);
        SmallestRectangle2(Region, &Row, &Column, &Value, &Length1, &Length2);

        if (0 != (Value>(PI/2)))
        {
            RotateImage(Img, &ImageRotate, (((PI/2)-Value)*180)/PI, "constant");
        }
        else
        {
            RotateImage(Img, &ImageRotate, ((-Value)*180)/PI, "constant");
        }
        FindShapeModel(ImageRotate, ModelId, HTuple(0).TupleRad(), HTuple(180).TupleRad(),
                       0.5, 1, 0.5, "least_squares", 0, 0.9, &Row, &Column, &Angle, &Score);


        if(Score.D()>0.8)
        {
			qDebug() << Score.D() << "....";
            VectorAngleToRigid(0, 0, 0, Row, Column, Angle, &HomMat2D);
            AffineTransContourXld(ModelContours, &ContoursAffineTrans, HomMat2D);
            TupleAbs(Angle, &Angle);
            Threshold(ImageRotate, &Region1, 0, para17);
            Connection(Region1,&Region1);
            SelectShapeStd(Region1, &Region1, "max_area", 70);
            RegionFeatures(Region1, "row1", &Value1);
            RegionFeatures(Region1, "column1", &Value2);
            RegionFeatures(Region1, "height", &Value3);
            if(ProIndex==1)
            {


                {
                    HTuple end_val36 = Value1+60;
                    HTuple step_val36 = 1;
                    for (Index=Value1+20; Index.Continue(end_val36, step_val36); Index += step_val36)
                    {
                        GenRectangle1(&Rectangle, Index, Value2, Index+1, Value2+10);
                        Intersection(Rectangle, Region1, &RegionIntersection);
                        SmallestRectangle1(RegionIntersection, &R, &C, &Row2, &Column2);
                        if (0 != (C==Value2))
                        {
                            break;
                        }

                    }

                }

				GenRectangle2(&ROI_1, Row, Column + 390, 0, 100, 150);
				GenRectangle2(&ROI_2, Row + 200, Column + 100, 0, 200, 80);
				GenRectangle2(&ROI_3, Row - 200, Column + 10, 0, 300, 80);
            }
        

            ReduceDomain(ImageRotate, ROI_1, &ImageReduced1);
            EdgesSubPix(ImageReduced1, &Edges1, "canny", 3, 20, 40);
			SelectContoursXld(Edges1, &Edges1, "contour_length", 300, 1000, -0.5, 0.5);
            GenRegionContourXld(Edges1, &ERegion1, "filled");

            SelectShapeStd(ERegion1, &ERegion1, "max_area", 70);
            JunctionsSkeleton(ERegion1, &EndPoints1, &JuncPoints1);
            Connection(EndPoints1, &ConnectedRegions1);
            RegionFeatures(ConnectedRegions1, "row", &r1);
            RegionFeatures(ConnectedRegions1, "column", &c1);

            line1.Clear();
            line1.Append(HTuple(r1[0]));
            line1.Append(HTuple(c1[0]));
            line1.Append(HTuple(r1[1]));
            line1.Append(HTuple(c1[1]));


            ReduceDomain(ImageRotate, ROI_2, &ImageReduced2);
            EdgesSubPix(ImageReduced2, &Edges2, "canny", 3, 20, 40);
			SelectContoursXld(Edges2, &Edges2, "contour_length", 300, 1000, -0.5, 0.5);
            GenRegionContourXld(Edges2, &ERegion2, "filled");
            SelectShapeStd(ERegion2, &ERegion2, "max_area", 70);
            JunctionsSkeleton(ERegion2, &EndPoints2, &JuncPoints2);

            Connection(EndPoints2, &ConnectedRegions2);
            RegionFeatures(ConnectedRegions2, "row", &r2);
            RegionFeatures(ConnectedRegions2, "column", &c2);

            line2.Clear();
            line2.Append(HTuple(r2[0]));
            line2.Append(HTuple(c2[0]));
            line2.Append(HTuple(r2[1]));
            line2.Append(HTuple(c2[1]));


            ReduceDomain(ImageRotate, ROI_3, &ImageReduced3);
            EdgesSubPix(ImageReduced3, &Edges3, "canny", 3, 20, 40);
			SelectContoursXld(Edges3, &Edges3, "contour_length", 300, 1000, -0.5, 0.5);
            GenRegionContourXld(Edges3, &ERegion3, "filled");
            SelectShapeStd(ERegion3, &ERegion3, "max_area", 70);

            JunctionsSkeleton(ERegion3, &EndPoints3, &JuncPoints3);

            Connection(EndPoints3, &ConnectedRegions3);
            RegionFeatures(ConnectedRegions3, "row", &r3);
            RegionFeatures(ConnectedRegions3, "column", &c3);

            line3.Clear();
            line3.Append(HTuple(r3[0]));
            line3.Append(HTuple(c3[0]));
            line3.Append(HTuple(r3[1]));
            line3.Append(HTuple(c3[1]));





            CreateMetrologyModel(&MetrologyHandle1);
            AddMetrologyObjectLineMeasure(MetrologyHandle1, HTuple(line1[0]), HTuple(line1[1]),
                    HTuple(line1[2]), HTuple(line1[3]), 20, 5, para19, para20, HTuple(), HTuple(),
                    &Index1);
            ApplyMetrologyModel(ImageRotate, MetrologyHandle1);
            GetMetrologyObjectResult(MetrologyHandle1, "all", "all", "result_type", "all_param",
                                     &Line1);
            ClearMetrologyModel(MetrologyHandle1);



            CreateMetrologyModel(&MetrologyHandle2);
            AddMetrologyObjectLineMeasure(MetrologyHandle2, HTuple(line2[0]), HTuple(line2[1]),
                    HTuple(line2[2]), HTuple(line2[3]), 20, 5, para19, para20, HTuple(), HTuple(),
                    &Index1);
            ApplyMetrologyModel(ImageRotate, MetrologyHandle2);
            GetMetrologyObjectResult(MetrologyHandle2, "all", "all", "result_type", "all_param",
                                     &Line2);

            ClearMetrologyModel(MetrologyHandle2);



            CreateMetrologyModel(&MetrologyHandle3);
            AddMetrologyObjectLineMeasure(MetrologyHandle3, HTuple(line3[0]), HTuple(line3[1]),
                    HTuple(line3[2]), HTuple(line3[3]), 20, 5, para19, para20, HTuple(), HTuple(),
                    &Index1);
            ApplyMetrologyModel(ImageRotate, MetrologyHandle3);
            GetMetrologyObjectResult(MetrologyHandle3, "all", "all", "result_type", "all_param",
                                     &Line3);

            ClearMetrologyModel(MetrologyHandle3);



            DistancePl(HTuple(Line2[0]), HTuple(Line2[1]), HTuple(Line3[0]), HTuple(Line3[1]),
                    HTuple(Line3[2]), HTuple(Line3[3]), &D2);
            DistancePl(HTuple(Line2[2]), HTuple(Line2[3]), HTuple(Line3[0]), HTuple(Line3[1]),
                    HTuple(Line3[2]), HTuple(Line3[3]), &D3);
            //垂直高度
            Distance2 = (D2+D3)/2;
            qDebug()<<Distance2.D()<<".....";
            Distance2=Distance2*para1+para3;
            if(!((Distance2<para4+para5)&&(Distance2>para4-para6)))
            {
                flag=1;
            }

            //水平长度
            DistancePl(R, C, HTuple(Line1[0]), HTuple(Line1[1]), HTuple(Line1[2]),
                    HTuple(Line1[3]), &Distance1);
            qDebug()<<Distance1.D()<<"....";
            Distance1=Distance1*para2+para7;
            if(!((Distance1<para8+para9)&&(Distance1>para8-para10)))
            {
                flag=1;
            }

            OutPara.Append(Distance2).Append(Distance1);
            return flag;
		}
        else
        {
            flag = 2;
            return flag;

        }
    }
    catch(HException ex)
    {
        OutPara.Append(0).Append(0);
        flag=2;
        return 2;
    }
    */


}
